{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Testing the Left Arm"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Warning:** Before running this notebook, first make sure you understand the command you run and make sure that the robot can freely move."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Note:** Also stop all other running Python script or notebook connected to the robot as only one connection can run at the same time."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib notebook\n",
    "\n",
    "import time\n",
    "import numpy as np\n",
    "\n",
    "from matplotlib import pyplot as plt"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Connect to the left arm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from reachy import Reachy, parts"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy = Reachy(\n",
    "    left_arm=parts.LeftArm(io='/dev/ttyUSB*', hand='force_gripper'),\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Check motors"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Check that all motors are presents"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for m in reachy.left_arm.motors:\n",
    "    print(f'{m.name} - pos:{m.present_position} temp:{m.temperature}')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Move the arm and check that the position are moving"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Turn stiff/compliant"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for m in reachy.left_arm.motors:\n",
    "    m.compliant = False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for m in reachy.left_arm.motors:\n",
    "    m.compliant = True"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Check that all motors are turning compliant/stiff."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Goto 0 pos + 90 on the elbow"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for m in reachy.left_arm.motors:\n",
    "    m.compliant = False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.goto({\n",
    "    'left_arm.shoulder_pitch': 0,\n",
    "    'left_arm.shoulder_roll': 0,\n",
    "    'left_arm.arm_yaw': 0,    \n",
    "    'left_arm.elbow_pitch': -90,\n",
    "    'left_arm.hand.forearm_yaw': 0,\n",
    "    'left_arm.hand.wrist_pitch': 0,\n",
    "    'left_arm.hand.wrist_roll': 0,\n",
    "    'left_arm.hand.gripper': 0,\n",
    "}, duration=3, wait=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Check that everything seems fine."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for m in reachy.left_arm.motors:\n",
    "    m.compliant = True"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Record a trajectory and replay it"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from reachy.trajectory import TrajectoryRecorder, TrajectoryPlayer"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "recorder = TrajectoryRecorder(reachy.left_arm.motors)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will record a move for 10sec. Make sure you hold the robot away from all objects before starting the record."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "recorder.start()\n",
    "time.sleep(10)\n",
    "recorder.stop()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Turn all motors stiff (try putting the robot in about its starting position of the trajectory)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for m in reachy.left_arm.motors:\n",
    "    m.compliant = False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "player = TrajectoryPlayer(reachy, recorder.trajectories)\n",
    "player.play(wait=True, fade_in_duration=3)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Turn it back compliant."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for m in reachy.left_arm.motors:\n",
    "    m.compliant = True"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Look at the recorded traj."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.figure()\n",
    "\n",
    "for k, v in recorder.trajectories.items():\n",
    "    plt.plot(v, label=k)\n",
    "# plt.legend()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Grasp objects"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Put an object inside the hand of the robot and then run the line below to close it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.left_arm.hand.gripper.compliant = False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.left_arm.hand.close()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Check that the close method returned true. Check the object holds in its hand while moving the arm manually."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then we reopen the hand."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.left_arm.hand.open()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.left_arm.hand.gripper.compliant = True"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Check kinematics"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Put the arm in a position where its hand is facing forward and where it can move at least 10cm in all direction."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for m in reachy.left_arm.motors:\n",
    "    m.compliant = False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "J0 = [m.present_position for m in reachy.left_arm.motors]\n",
    "\n",
    "M = reachy.left_arm.forward_kinematics(J0)\n",
    "\n",
    "M1 = M.copy()\n",
    "M1[0, 3] += 0.05\n",
    "J1 = reachy.left_arm.inverse_kinematics(M1, J0)\n",
    "\n",
    "M2 = M.copy()\n",
    "M2[0, 3] -= 0.05\n",
    "J2 = reachy.left_arm.inverse_kinematics(M2, J0)\n",
    "\n",
    "M3 = M.copy()\n",
    "M3[1, 3] += 0.05\n",
    "J3 = reachy.left_arm.inverse_kinematics(M3, J0)\n",
    "\n",
    "M4 = M.copy()\n",
    "M4[1, 3] -= 0.05\n",
    "J4 = reachy.left_arm.inverse_kinematics(M4, J0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "_ = reachy.goto({m.name: j for j, m in zip(J1, reachy.left_arm.motors)}, duration=1, wait=True)\n",
    "_ = reachy.goto({m.name: j for j, m in zip(J2, reachy.left_arm.motors)}, duration=1, wait=True)\n",
    "_ = reachy.goto({m.name: j for j, m in zip(J3, reachy.left_arm.motors)}, duration=1, wait=True)\n",
    "_ = reachy.goto({m.name: j for j, m in zip(J4, reachy.left_arm.motors)}, duration=1, wait=True)\n",
    "_ = reachy.goto({m.name: j for j, m in zip(J0, reachy.left_arm.motors)}, duration=1, wait=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for m in reachy.left_arm.motors:\n",
    "    m.compliant = True"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Check the fans"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.left_arm.shoulder_fan.on()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.left_arm.shoulder_fan.off()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.left_arm.elbow_fan.on()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.left_arm.elbow_fan.off()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.left_arm.wrist_fan.on()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.left_arm.wrist_fan.off()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
